Mapping of environment, Detection and Tracking of Moving Objects using Occupancy Grids

In this paper, we present a real-time algorithm for local simultaneous localization and mapping (SLAM) with detection and tracking of moving objects (DATMO) in dynamic outdoor environments from a moving vehicle equipped with a laser scanner and two radars. To correct vehicle location from odometry we introduce a new fast implementation of incremental scan matching method that can work reliably in dynamic outdoor environments. After a good vehicle location is estimated, the surrounding map is updated incrementally and moving objects are detected without a priori knowledge of the targets. Detected moving objects are finally tracked by a Multiple Hypothesis Tracker (MHT) coupled with an adaptive IMM (Interacting Multiple Models) Filter. The experimental results on datasets collected from different scenarios such as: urban streets, country roads and highways demonstrate the efficiency of the proposed algorithm on a Daimler Mercedes demonstrator in the framework of the European Project PReVENT-ProFusion2.